#!/usr/bin/python
# -*- coding: utf-8 -*-

# Ce programme utilise les 8 sorties digitales de la carte Velleman k8055 / VM110
# pour piloter deux moteurs pas à pas unipolaires.
# Il supporte la rotation d'un nombre de pas défini dans les deux sens
# et dans trois modes (Single step, Full step, Half step).
#
# Ce programme a été transcodé depuis l'original en C et conserve strictement la 
# même structure afin d'être comparable avec ce dernier. Il n'est pas optimisé 
# pour Python, c'est volontaire.
#
# David Mézière <dmeziere@gmail.com>

# py-stepper.py

import sys
import time
from pyk8055 import *

class Stepper():
    
    # Tableau des séquences. Les dimensions sont le mode, le moteur, et le pas.
    # Certains modes ont 8 pas, d'autres 4. Ceux de 4 pas ont étés doublés afin
    # de répondre au besoin du langage d'avoir des tableaux uniformes. Les 3
    # dimensions sont mode, moteur et pas.
    aSequences = [

        # Mode single step, activation d'une seule phase par pas.
        # Plus faible consommation, plus faible couple.
        [
            [0, 0, 0, 0, 0, 0, 0, 0],               # Inutilisé, présent par compatibilité.
            [1, 2, 4, 8, 1, 2, 4, 8],               # Séquence du moteur 1.
            [16, 32, 64, 128, 16, 32, 64, 128],     # Séquence du moteur 2.
            [17, 34, 68, 136, 17, 34, 68, 136],     # Moteur 1 et 2 dans le même sens.
            [129, 66, 36, 24, 129, 66, 36, 24]      # Moteur 1 et 2 en sens inverse.
        ],

        # Mode full step, activation des phases par paire.
        # Consommation doublée par rapport au single step, plus fort couple.
        [
            [0, 0, 0, 0, 0, 0, 0, 0],               # Inutilisé, présent par compatibilité.
            [3, 6, 12, 9, 3, 6, 12, 9],             # Séquence du moteur 1.
            [48, 96, 192, 144, 48, 96, 192, 144],   # Séquence du moteur 2.
            [51, 102, 204, 153, 51, 102, 204, 153], # Moteur 1 et 2 dans le même sens.
            [195, 102, 60, 153, 195, 102, 60, 153]  # Moteur 1 et 2 en sens inverse.
        ],

        # Mode half step, activation des phases une par une, puis par paire.
        # Consommation doublée par rapport au single step, plus fort couple.
        # Précision doublée par activation des demi-pas.
        # Vitesse divisée par deux, de par le doublement des pas.
        [
            [0, 0, 0, 0, 0, 0, 0, 0],               # Inutilisé, présent par compatibilité.
            [1, 3, 2, 6, 4, 12, 8, 9],              # Séquence du moteur 1.
            [16, 48, 32, 96, 64, 192, 128, 144],    # Séquence du moteur 2.
            [17, 51, 34, 102, 68, 204, 136, 153],   # Moteur 1 et 2 dans le même sens.
            [129, 195, 66, 102, 36, 60, 24, 153]    # Moteur 1 et 2 en sens inverse.
        ]
    ]
    
    # Sequence length.
    iSeqLength = 8

    # Last step used on each motor.
    iLastPhase = [0, 0, 0, 0, 0]

    # Last sense used on each motor.
    iLastSense = [0, 0, 0, 0, 0]

    sModeNames = ["Single step", "Full step", "Half step"]

    # Default parameters.
    iVerbose = False
    iPort = 0
    iMode = 0

    iStart = 0
    
    def __init__(self):
        
        self.iStart = self.time()

        # The switches that don't need to open the board are checked first.
        for i in range(1, len(sys.argv)) :

            # Usage informations
            if sys.argv[i] == "-h" or sys.argv[i] == "--help" :
                self.displayHelp(sys.argv)
                exit(0)

            # Verbose mode
            elif sys.argv[i] == "-v" or sys.argv[i] == "--verbose" :
                print "[%05d] Entering verbose mode." % self.time()
                self.iVerbose = True

            # Port number
            elif sys.argv[i][:3] == "-p:" :
                if not self.setPort(sys.argv[i][3:]) :
                    exit(1)
            elif sys.argv[i][:7] == "--port:" :
                if not self.setPort(sys.argv[i][7:]) :
                    exit(1)

            # Stepping mode
            elif sys.argv[i][:3] == "-m:" :
                if not self.setMode(sys.argv[i][3:]) :
                    exit(1)
            elif sys.argv[i][:7] == "--mode:" :
                if not self.setMode(sys.argv[i][:7]) :
                    exit(1)

        # Opening of the board
        try:
            self.card = k8055(self.iPort, False)
        except IOError:
            print "Unable to open the k8055 board (port: %d)." % self.iPort
            exit(1)

        if self.iVerbose :
            print "[%05d] Opening the k8055 board (port: %d)." % (self.time(), self.iPort)

        # The switches that need to open the board are then checked.
        for i in range(1, len(sys.argv)) :

            # Usage informations
            if sys.argv[i] == "-h" or sys.argv[i] == "--help" :
                # This switch has already been checked.
                # But we must not throw an error.
                pass

            # Verbose mode
            elif sys.argv[i] == "-v" or sys.argv[i] == "--verbose" :
                # This switch has already been checked.
                # But we must not throw an error.
                pass

            # Port number
            elif sys.argv[i][:3] == "-p:" or sys.argv[i][:7] == "--port:" :
                # This switch has already been checked.
                # But we must not throw an error.
                pass

            # Stepping mode
            elif sys.argv[i][:3] == "-m:" or sys.argv[i][:7] == "--mode:" :
                # This switch has already been checked.
                # But we must not throw an error.
                pass

            # Motor 1 steps.
            elif sys.argv[i][:3] == "-1:" :
                self.setSteps(1, sys.argv[i][3:])
            elif sys.argv[i][:9] == "--motor1:" :
                self.setSteps(1, sys.argv[i][9:])

            # Motor 2 steps.
            elif sys.argv[i][:3] == "-2:" :
                self.setSteps(2, sys.argv[i][3:])
            elif sys.argv[i][:9] == "--motor2:" :
                self.setSteps(2, sys.argv[i][9:])

            # Both motors steps.
            elif sys.argv[i][:3] == "-b:" :
                self.setSteps(3, sys.argv[i][3:])
            elif sys.argv[i][:7] == "--both:" :
                self.setSteps(3, sys.argv[i][7:])

            # Both motors steps, reversed.
            elif sys.argv[i][:3] == "-r:" :
                self.setSteps(4, sys.argv[i][3:])
            elif sys.argv[i][:11] == "--reversed:" :
                self.setSteps(4, sys.argv[i][11:])

            else :
                print "Unknown option."
                exit(1)

        # Putting down all outputs.
        self.card.WriteAllDigital(0)

        self.card.CloseDevice()

        if self.iVerbose :
            print "[%05d] Closing the k8055 board (port: %d)." % (self.time(), self.iPort)

    def time(self):
        myTime = int(time.time() * 1000) - self.iStart
        return int(myTime)

    """
    Write help to standard output.

    @param char *params Parameters array.

    @return True if ok (always).
    """
    def displayHelp(self, params):

        print "%s" % params[0]
        print "   -h, --help: Displays these informations."
        print "   -v, --verbose: Display the orders as they are executed."
        print "   -p, --port: K8055 board address."
        print "      0: SK5 and SK6 are closed on the board (Default)."
        print "      1: SK5 is open and SK6 is closed on the board."
        print "      2: SK5 is closed and SK6 is open on the board."
        print "      3: SK5 and SK6 are open on the board."
        print "   -m, --mode: Motor phases activation mode."
        print "      0: Single step mode. Activation of one phase at once. Weaker consumption and couple (Par défaut)."
        print "      1: Full step mode. Activation of phases per pair. Consumption and couple doubled."
        print "      2: Half step mode. Mix of the two preceedings. Consumption, couple and precision doubled."
        print "   -1, --motor1: Number of steps for motor 1."
        print "   -2, --motor2: Number of steps for motor 2."
        print "   -b, --both: Number of steps for motor 1, motor 2 will follow the same way."
        print "   -r, --reversed: Number of steps for motor 1, motor 2 will follow the opposite way."
        print ""
        print "A step count can be positive or negative. In reversed mode, you drive motor 1, motor 2 will be reversed."
        print ""
        print "The -h, -v, -p and -m switches must be used first and once. But the others can be duplicated."
        print "You cannot drive multiple boards with a single command. But a script can send multiple command lines."
        print "Example: %s --motor1:+50 --motor2:-50 --motor1:-50 --motor2:+50 --both:+100 --both:-100" % params[0]

        return True

    """
    Sets the board port.

    @param char* value String value to be converted as int.

    @return int 1 if ok, 0 if ko.
    """
    def setPort(self, value):

        value = int(value)

        # The port must be between 0 and 3 included.
        if value < 0 or value > 3 :
            print "Incorrect port: %d." % value
            return False

        # If everything's ok the new value is applyed.
        self.iPort = value

        # If in verbose mode, the new mode is displayed.
        if self.iVerbose :
            print "[%05d] Switching to port %d." % (self.time(), self.iPort)

        return True

    """
    Sets the stepping mode.

    @param char* value String value to be converted as int.

    @return int 1 if ok, 0 if ko.
    """
    def setMode(self, value):

        value = int(value)

        # The mode must be between 0 and 2 included.
        if value < 0 or value > 2 :
            print "Incorrect mode: %d." % value
            return False

        # If everything's ok the new value is applyed.
        self.iMode = value

        # If in verbose mode, the new mode is displayed.
        if self.iVerbose :
            print "[%05d] Switching to %s mode." % (self.time(), self.sModeNames[self.iMode])

        return True

    """
    Sends the steps sequences to the card.
    
    @param int iMotor Motor index
    @param int iQuantity Quantity of steps to send
    
    @return int 1 if ok, 0 if ko.
    """
    def setSteps(self, iMotor, iQuantity) :

        iMotor = int(iMotor)
        iQuantity = int(iQuantity)

        # iMotor must be between 1 and 4 included.
        if iMotor < 1 or iMotor > 4 :
            print "Invalid motor number."
            return False

        # Output of the order.
        if self.iVerbose :

            if iMotor == 1 or iMotor == 2 :
                print "[%05d] Motor %d: %+d steps." % (self.time(), iMotor, iQuantity)
            elif iMotor == 3 :
                print "[%05d] Motor 1: %+d steps. Motor 2: %+d steps." % (self.time(), iQuantity, iQuantity)
            elif iMotor == 4 :
                print "[%05d] Motor 1: %+d steps. Motor 2: %+d steps." % (self.time(), iQuantity, -iQuantity)

        iStep = 0
        iStepPhase = self.iLastPhase[iMotor]

        if self.iLastSense[iMotor] < 0 and iQuantity >= 0 :
            iStepPhase += 1
        elif self.iLastSense[iMotor] == 0 and iQuantity < 0 :
            iStepPhase -= 1

        while iStep < abs(iQuantity) :

            if iQuantity < 0 :

                iStepPhase -= 1

                if iStepPhase < 0 :
                    iStepPhase = self.iSeqLength + iStepPhase

            else :

                if iStepPhase == self.iSeqLength :
                    iStepPhase = 0

            # Envoi de la valeur calculée à la carte k8055.
            self.card.WriteAllDigital(self.aSequences[self.iMode][iMotor][iStepPhase])

            # Incrementing or decrementing the steps, as asked.
            if iQuantity >= 0 :

                iStepPhase += 1

                if iStepPhase == self.iSeqLength :
                    iStepPhase = 0

            else :

                if iStepPhase < 0 :
                    iStepPhase = self.iSeqLength - 1

            iStep += 1

        # Reminding the laste rotaton sense.
        if iQuantity >= 0 :
            self.iLastSense[iMotor] = 0
        else :
            self.iLastSense[iMotor] = - 1

        # Reminding the state of the motor for next use.
        self.iLastPhase[iMotor] = iStepPhase

        return True

if __name__ == "__main__":
    Stepper()
